#! usr/bin/env python
import rospy
from turtlesim.msg import *
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
#发布方：订阅乌龟的位置信息，转换成坐标系的相对关系，并发布
#准  备：  话题：/turtle1/pose  类型：turtlesim/Pose
#流程：
#   1.导入包  2.初始化订阅节点  3.创建订阅对象  4.将位置信息转换为坐标系之间的信息  5.发布信息

def doPose(pose):
    #这是订阅到信息的回调函数
    #创建发布坐标系的相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    #将pose值转换成坐标系相对关系的消息
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.child_frame_id = "turtle1"
    ts.header.stamp= rospy.Time.now()
    #声明乌龟坐标系对于父坐标系的偏移量等
    ts.transform.translation.x =  pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0.0
    #四元数
    #从欧拉角转换
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]
    #发布
    pub.sendTransform(ts)

if __name__ == "__main__":
    rospy.init_node("dynamic_pub_p")
    #创建订阅者对象，以订阅乌龟的位置信息
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100) 
    rospy.spin()